﻿using System;
using System.Collections.Generic;
using System.Windows;
using System.Windows.Media;

namespace RoadPlot
{
    //http://www.codeproject.com/script/Articles/ViewDownloads.aspx?aid=31859

    public static class BezierSpline
    {
        public static void AddSegments(List<Point> curve, PathSegmentCollection segments)
        {
            if (curve.Count < 2)
            {
                return;
            }
            Point[] cp1;
            Point[] cp2;
            
            GetCurveControlPoints(curve.ToArray(), out cp1, out cp2);

            for (var x = 0; x < curve.Count - 1; x++)
            {
                segments.Add(new BezierSegment(cp1[x], cp2[x], curve[x + 1], true));
            }
        }

        /// <summary>
        /// Get open-ended Bezier Spline Control Points.
        /// </summary>
        /// <param name="knots">Input Knot Bezier spline points.</param>
        /// <param name="firstControlPoints">Output First Control points array of knots.Length - 1 length.</param>
        /// <param name="secondControlPoints">Output Second Control points array of knots.Length - 1 length.</param>
        /// <exception cref="ArgumentNullException"><paramref name="knots"/> parameter must be not null.</exception>
        /// <exception cref="ArgumentException"><paramref name="knots"/> array must containg at least two points.</exception>
        private static void GetCurveControlPoints(Point[] knots, out Point[] firstControlPoints, out Point[] secondControlPoints)
        {
            if (knots == null)
                throw new ArgumentNullException("knots");

            int n = knots.Length - 1;

            if (n < 1)
                throw new ArgumentException("At least two knot points required", "knots");

            if (n == 1)
            { // Special case: Bezier curve should be a straight line.
                firstControlPoints = new Point[1];
                // 3P1 = 2P0 + P3
                firstControlPoints[0].X = (2 * knots[0].X + knots[1].X) / 3;
                firstControlPoints[0].Y = (2 * knots[0].Y + knots[1].Y) / 3;

                secondControlPoints = new Point[1];
                // P2 = 2P1 – P0
                secondControlPoints[0].X = 2 * firstControlPoints[0].X - knots[0].X;
                secondControlPoints[0].Y = 2 * firstControlPoints[0].Y - knots[0].Y;
                return;
            }

            // Calculate first Bezier control points
            // Right hand side vector
            var rhs = new double[n];

            // Set right hand side X values
            for (int i = 1; i < n - 1; ++i)
            {
                rhs[i] = 4 * knots[i].X + 2 * knots[i + 1].X;
            }

            rhs[0] = knots[0].X + 2 * knots[1].X;
            rhs[n - 1] = (8 * knots[n - 1].X + knots[n].X) / 2.0;

            // Get first control points X-values
            var x = GetFirstControlPoints(rhs);

            // Set right hand side Y values
            for (int i = 1; i < n - 1; ++i)
            {
                rhs[i] = 4*knots[i].Y + 2*knots[i + 1].Y;
            }

            rhs[0] = knots[0].Y + 2 * knots[1].Y;
            rhs[n - 1] = (8 * knots[n - 1].Y + knots[n].Y) / 2.0;

            // Get first control points Y-values
            var y = GetFirstControlPoints(rhs);

            // Fill output arrays.
            firstControlPoints = new Point[n];
            secondControlPoints = new Point[n];
            for (int i = 0; i < n; ++i)
            {
                // First control point
                firstControlPoints[i] = new Point(x[i], y[i]);
                // Second control point
                if (i < n - 1)
                    secondControlPoints[i] = new Point(2 * knots[i + 1].X - x[i + 1], 2 * knots[i + 1].Y - y[i + 1]);
                else
                    secondControlPoints[i] = new Point((knots[n].X + x[n - 1]) / 2, (knots[n].Y + y[n - 1]) / 2);
            }
        }

        /// <summary>
        /// Solves a tridiagonal system for one of coordinates (x or y) of first Bezier control points.
        /// </summary>
        /// <param name="rhs">Right hand side vector.</param>
        /// <returns>Solution vector.</returns>
        private static double[] GetFirstControlPoints(double[] rhs)
        {
            int n = rhs.Length;
            var x = new double[n]; // Solution vector.
            var tmp = new double[n]; // Temp workspace.

            double b = 2.0;
            x[0] = rhs[0] / b;
            for (int i = 1; i < n; i++) // Decomposition and forward substitution.
            {
                tmp[i] = 1 / b;
                b = (i < n - 1 ? 4.0 : 3.5) - tmp[i];
                x[i] = (rhs[i] - x[i - 1]) / b;
            }

            for (int i = 1; i < n; i++)
            {
                x[n - i - 1] -= tmp[n - i]*x[n - i]; // Backsubstitution.
            }

            return x;
        }
    }
}
